from ulab import numpy as np
from media.sensor import *
from media.display import *
from media.media import *
import image
import time,sys,os,utime

sensor_id=2
blue_threshold=(0, 81, 1, 127, -128, -25)

# 显示模式选择：可以是 "VIRT"、"LCD" 或 "HDMI"
DISPLAY_MODE = "VIRT"

# 根据模式设置显示宽高
if DISPLAY_MODE == "VIRT":
    # 虚拟显示器模式
    DISPLAY_WIDTH = 320
    DISPLAY_HEIGHT = 240
elif DISPLAY_MODE == "LCD":
    # 3.1寸屏幕模式
    DISPLAY_WIDTH = 800
    DISPLAY_HEIGHT = 480
elif DISPLAY_MODE == "HDMI":
    # HDMI扩展板模式
    DISPLAY_WIDTH = 1920
    DISPLAY_HEIGHT = 1080
else:
    raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")

def Display_Init():
    if DISPLAY_MODE == "VIRT":
        Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=30)
    elif DISPLAY_MODE == "LCD":
        Display.init(Display.ST7701,width=DISPLAY_WIDTH,height=DISPLAY_HEIGHT,to_ide=False)
    elif DISPLAY_MODE == "HDMI":
        Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
    else:
        raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")



#寻找最大方块
def find_max(rects):
    max_size=0
    max_rect=None
    for rect in rects:
        if rect.w()*rect.h()>max_size:
            max_rect=rect
            max_size=rect.w()*rect.h()
    return max_rect

def intersection(x1, y1, x2, y2, x3, y3, x4, y4):
    """输入四个坐标，计算前两个点组成的线段，后两个点组成的线段的交点坐标"""
    if x2 - x1 != 0 and x4 - x3 != 0:  # 都不垂直
        k1 = (y2 - y1) / (x2 - x1)
        b1 = y1 - k1 * x1

        k2 = (y4 - y3) / (x4 - x3)
        b2 = y3 - k2 * x3

        if k1 - k2 == 0:  # 平行无交点
            return None
        else:
            x = (b2 - b1) / (k1 - k2)
            y = k1 * x + b1
            return (int(x), int(y))
    elif x2 - x1 == 0 and x4 - x3 != 0:  # 第一条垂直
        k2 = (y4 - y3) / (x4 - x3)
        b2 = y3 - k2 * x3
        x = x1
        y = k2 * x + b2
        return (int(x), int(y))
    elif x2 - x1 != 0 and x4 - x3 == 0:  # 第二条垂直
        k1 = (y2 - y1) / (x2 - x1)
        b1 = y1 - k1 * x1
        x = x3
        y = k1 * x + b1
        return (int(x), int(y))
    else:  # 都垂直
        if x1 == x3:  # 同一条线
            return (x1, int((y1 + y2 + y3 + y4) / 4))  # 返回中点
        else:  # 平行无交点
            return (0, 0)


def find_chessboard(img):
    use_img=img.copy()
    use_img=use_img.binary([blue_threshold]).close(3)

    rects=use_img.find_rects(threshold=1000)
    rect=find_max(rects)
    return rect






try:
    #传感器初始化
    sensor=Sensor(id=sensor_id,width=DISPLAY_WIDTH,height=DISPLAY_HEIGHT)
    #复位
    sensor.reset()

    #设置输出尺寸和通道
    sensor.set_framesize(width=DISPLAY_WIDTH,height=DISPLAY_HEIGHT,chn=CAM_CHN_ID_1)
    #设置输出画面格式
    sensor.set_pixformat(Sensor.RGB565,CAM_CHN_ID_1)
    #是否水平翻转
    sensor.set_hmirror(False)
    #是否垂直翻转
    sensor.set_vflip(False)

    #显示设备初始化
    Display_Init()

    #媒体堆栈初始化
    MediaManager.init()

    sensor.run()

    #帧率显示对象初始化
    clock=utime.clock()

    while True:
        os.exitpoint()

        #更新当前时间
        clock.tick()


        #截取当前画面
        img=sensor.snapshot(CAM_CHN_ID_1)

        rect=find_chessboard(img)
        if rect:
            ((x1,y1),(x2,y2),(x3,y3),(x4,y4))=rect.corners()
            img.draw_line(x1,y1,x2,y2,color=(0,255,0),thickness=2)
            img.draw_line(x2,y2,x3,y3,color=(0,255,0),thickness=2)
            img.draw_line(x3,y3,x4,y4,color=(0,255,0),thickness=2)
            img.draw_line(x4,y4,x1,y1,color=(0,255,0),thickness=2)
#            img.draw_rectangle(rect.rect(),color=(0,255,0),thickness=2)
            print("rect: ",rect.rect())
        #显示当前画面
        Display.show_image(img)
        print("fps: ",clock.fps())



except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    # 停止传感器运行
    if isinstance(sensor, Sensor):
        sensor.stop()
    # 反初始化显示模块
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    # 释放媒体缓冲区
    MediaManager.deinit()


